#ifndef __MPU6050DEVICE_H__
#define __MPU6050DEVICE_H__

#include "plat/nonecopyable.h"
#include "device/softpointpwm.h"
#include "soc/soc_gpio.h"
#include "device_def.h"
#include "os/mutex.h"
#include "lib/stringex.h"
#include "soc/soc_exti.h"
#include "common/mpu6050devicedef.h"
#include "alg/imu/MahonyAHRS.h"
#include "alg/imu/MadgwickAHRS.h"

//默认采样率
#define MPU6050Device_DEFAULT_RATE 200

/// @brief MPU6050
class MPU6050Device : public NoneCopyable
{
    using INT_FUNC = std::function<void(void)>;

public:
    explicit MPU6050Device();
    virtual ~MPU6050Device();

public:
    void ConfigEmuIIC(const GpioPortPin &scl, const GpioPortPin &sda);
    void ConfigHardware(const GpioPortPin &ad, GpioExtiDevId extiId = GpioExtiDevId::None);
    void Config(MPU6050IICMode iicMode, MPU6050CalcMode calcMode, 
                int rate = MPU6050Device_DEFAULT_RATE, bool enInterrupt = false);
    
    void SetInterruptCallBack(const INT_FUNC &func);

    bool Init();
    
public:
    bool GetResult(float &pitch, float &rol, float &yaw);
    bool GetResult(MPU6050Result &result, 
                   bool readAcc = false, bool readGyr = false, bool readTemp = false);

    bool GetAcc(int16_t &accX, int16_t &accY, int16_t &accZ);
    bool GetGry(int16_t &gyrX, int16_t &gyrY, int16_t &gyrZ);
    bool GetTemp(float &temp);

    float ConvertRawAcceleration(int aRaw);
    float ConvertRawGyro(int gRaw);

private:
    void SwitchContext();
    bool GetResultNoDmpImpl(float &pitch, float &rol, float &yaw);

public:
    GpioPortPin m_SCLPortPin;
    GpioPortPin m_SDAPortPin;
    GpioPortPin m_ADPortPin;
    
    SOC_Exti m_exti;

    MPU6050IICMode m_iicMode = MPU6050IICMode::IIC_Emu;
    MPU6050CalcMode m_calcMode = MPU6050CalcMode::Mahony;
    int m_sampleRate = MPU6050Device_DEFAULT_RATE;
    bool m_interrupt = false;

    Mutex m_mutex;

    //
    Mahony m_mahonyAlg;
    Madgwick m_madgwickAlg;
};

#endif // __MPU6050DEVICE_H__
